#include "DriveTrain.h"
#include "../Robotmap.h"
#include "../Commands/DriveWithJoysticks.h"

DriveTrain::DriveTrain() : Subsystem("DriveTrain") {
	
	drive = new RobotDrive(new Jaguar(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::TRACTION_LEFT_MOTOR_1),
			               new Jaguar(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::TRACTION_LEFT_MOTOR_2),
			               new Jaguar(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::TRACTION_RIGHT_MOTOR_1),
			               new Jaguar(RobotMap::DIGITAL_MODULE_SLOT, RobotMap::TRACTION_RIGHT_MOTOR_2));
	
	drive->SetSafetyEnabled(false);
	precissionDriving = false;
	drivingMode = NORMAL_MODE;
}

void DriveTrain::InitDefaultCommand()
{
	SetDefaultCommand(new DriveWithJoysticks());
}

int DriveTrain::getDrivingMode() {
	return drivingMode;
}

void DriveTrain::setReversedMode() {
	drivingMode = REVERSED_MODE;
}

void DriveTrain::setNormalMode() {
	drivingMode = NORMAL_MODE;
}

void DriveTrain::setPrecissionDriving(bool b) {
	precissionDriving = b;
}

char* DriveTrain::getDrivingModeName() {
	switch(drivingMode) {
	case NORMAL_MODE:
		return "Normal Mode";
	case REVERSED_MODE:
		return "Reversed Mode";
	default:
		return "UnKnown";
	}
}

bool DriveTrain::isPrecissionDriving() {
	return precissionDriving;
}

void DriveTrain::stop() {
	drive->StopMotor();
}


void DriveTrain::tankDrive(float left, float right) {
	
	float leftS = 0.0, rightS = 0.0;
	
	switch(drivingMode) {
	case NORMAL_MODE:
		leftS = -left;
		rightS = -right;
		break;
	case REVERSED_MODE:
		leftS = right;
		rightS = left;
		break;
	}
	
	if(!precissionDriving)
		drive->TankDrive(leftS, rightS);
	else
		drive->TankDrive( leftS/2.0, rightS/2.0);
}

int DriveTrain::getOriginalSign(float x) {
	return x < 0.0 ? -1 : 1;
}






